{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "a76ababb",
   "metadata": {
    "colab_type": "text",
    "id": "TKvYiJgnYExi"
   },
   "source": [
    "This notebook provides examples to go along with the [textbook](http://manipulation.csail.mit.edu/intro.html).  I recommend having both windows open, side-by-side!\n",
    "\n",
    "## Instructions for running this notebook on Deepnote\n",
    "\n",
    "- Log in (the free account will be sufficient for this class)\n",
    "- \"Duplicate\" the document.  Icon is in the top right next to Login.\n",
    "- Read/run the cells one at a time OR use the \"Run notebook\" icon just above this cell\n",
    "- To open the visualizer, click on the url printed just below \"StartMeshcat\" in the second code cell of the notebook.\n",
    "- To teleop, use the keyboard commands printed in the third/fourth code cell outputs OR in meshcat, click \"Open controls\" and use the sliders.\n",
    "- When you're done, press the ESCAPE key OR click the \"Stop Simulation\" button in meshcat."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0cfd3a82",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "A4QOaw_zYLfI",
    "lines_to_end_of_cell_marker": 2
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.geometry import StartMeshcat\n",
    "from pydrake.multibody.inverse_kinematics import (\n",
    "    DifferentialInverseKinematicsParameters,\n",
    "    DifferentialInverseKinematicsStatus,\n",
    "    DoDifferentialInverseKinematics,\n",
    ")\n",
    "from pydrake.systems.analysis import Simulator\n",
    "from pydrake.systems.framework import DiagramBuilder, EventStatus, LeafSystem\n",
    "from pydrake.visualization import MeshcatPoseSliders\n",
    "\n",
    "from manipulation import running_as_notebook\n",
    "from manipulation.meshcat_utils import WsgButton\n",
    "from manipulation.scenarios import AddIiwaDifferentialIK, ExtractBodyPose\n",
    "from manipulation.station import MakeHardwareStation, load_scenario"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "a162e466",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start the visualizer.\n",
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7eadf3ee",
   "metadata": {
    "colab_type": "text",
    "id": "C1sdq2R88C16"
   },
   "source": [
    "# Teleop Example (2D)\n",
    "\n",
    "In this example, we assemble a diagram with all of the relevant subsystems (the manipulation station, the meshcat visualizer, and some systems that provide a minimal teleop interface and convert the teleop output from end-effector commands into joint commands.  We'll learn more about each of these components in the following chapters.\n",
    "\n",
    "**NOTE:** If you command the robot to move its gripper beyond what is possible, then you get a message about \"differential IK\" failing.  I've left that in for now (rather than setting very conservative slider limits) partly because it has tutorial value.  We'll understand it more precisely soon!  For now, just stop the simulation and rerun the cell if you get stuck.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "185d1d03",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "7dimkJhd9DjP"
   },
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_model:\n",
    "    name: iiwa\n",
    "    file: package://manipulation/planar_iiwa14_no_collision.urdf\n",
    "    default_joint_positions:\n",
    "        iiwa_joint_2: [0.1]\n",
    "        iiwa_joint_4: [-1.2]\n",
    "        iiwa_joint_6: [1.6]\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: iiwa::iiwa_link_0\n",
    "- add_model:\n",
    "    name: wsg\n",
    "    file: package://manipulation/hydro/schunk_wsg_50_with_tip.sdf\n",
    "- add_weld:\n",
    "    parent: iiwa::iiwa_link_7\n",
    "    child: wsg::body\n",
    "    X_PC:\n",
    "        translation: [0, 0, 0.09]\n",
    "        rotation: !Rpy { deg: [90, 0, 90]}\n",
    "- add_model:\n",
    "    name: foam_brick\n",
    "    file: package://manipulation/hydro/061_foam_brick.sdf\n",
    "- add_model:\n",
    "    name: robot_table\n",
    "    file: package://manipulation/hydro/extra_heavy_duty_table_surface_only_collision.sdf\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: robot_table::link\n",
    "    X_PC:\n",
    "        translation: [0, 0, -0.7645]\n",
    "- add_model:\n",
    "    name: work_table\n",
    "    file: package://manipulation/hydro/extra_heavy_duty_table_surface_only_collision.sdf\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: work_table::link\n",
    "    X_PC:\n",
    "        translation: [0.75, 0, -0.7645]\n",
    "# Restrict the brick to move only in the x-z plane\n",
    "- add_model:\n",
    "    name: planar_joint\n",
    "    file: package://manipulation/planar_joint.sdf\n",
    "    default_joint_positions:\n",
    "        planar_joint: [0.6, 0, 0]\n",
    "- add_weld:\n",
    "    parent: world\n",
    "    child: planar_joint::parent\n",
    "    X_PC:\n",
    "        rotation: !Rpy { deg: [90, 0, 0]}\n",
    "- add_weld:\n",
    "    parent: planar_joint::child\n",
    "    child: foam_brick::base_link\n",
    "model_drivers:\n",
    "    iiwa: !IiwaDriver\n",
    "      hand_model_name: wsg\n",
    "    wsg: !SchunkWsgDriver {}\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "def teleop_2d():\n",
    "    scenario = load_scenario(data=scenario_data)\n",
    "    meshcat.Set2dRenderMode(xmin=-0.25, xmax=1.5, ymin=-0.1, ymax=1.3)\n",
    "\n",
    "    builder = DiagramBuilder()\n",
    "\n",
    "    station = builder.AddSystem(MakeHardwareStation(scenario, meshcat))\n",
    "\n",
    "    # TODO(russt): Move this whole block into station.AddDiffIk(scenario, frame, builder)\n",
    "    # Set up differential inverse kinematics.\n",
    "    controller_plant = station.GetSubsystemByName(\n",
    "        \"iiwa.controller\"\n",
    "    ).get_multibody_plant_for_control()\n",
    "    differential_ik = AddIiwaDifferentialIK(\n",
    "        builder,\n",
    "        controller_plant,\n",
    "        frame=controller_plant.GetFrameByName(\"iiwa_link_7\"),\n",
    "    )\n",
    "    builder.Connect(\n",
    "        differential_ik.get_output_port(),\n",
    "        station.GetInputPort(\"iiwa.position\"),\n",
    "    )\n",
    "    builder.Connect(\n",
    "        station.GetOutputPort(\"iiwa.state_estimated\"),\n",
    "        differential_ik.GetInputPort(\"robot_state\"),\n",
    "    )\n",
    "\n",
    "    # Set up teleop widgets.\n",
    "    meshcat.DeleteAddedControls()\n",
    "    teleop = builder.AddSystem(\n",
    "        MeshcatPoseSliders(\n",
    "            meshcat,\n",
    "            lower_limit=[0, -np.pi, -np.pi, -0.6, -1, 0],\n",
    "            upper_limit=[2 * np.pi, np.pi, np.pi, 0.8, 1, 1.1],\n",
    "            # Only roll, x, and z are used in this example:\n",
    "            visible=[True, False, False, True, False, True],\n",
    "            decrement_keycodes=[\"KeyQ\", \"\", \"\", \"ArrowLeft\", \"\", \"ArrowDown\"],\n",
    "            increment_keycodes=[\"KeyE\", \"\", \"\", \"ArrowRight\", \"\", \"ArrowUp\"],\n",
    "        )\n",
    "    )\n",
    "    builder.Connect(\n",
    "        teleop.get_output_port(), differential_ik.GetInputPort(\"X_WE_desired\")\n",
    "    )\n",
    "    # Note: This is using \"Cheat Ports\". For it to work on hardware, we would\n",
    "    # need to construct the initial pose from the HardwareStation outputs.\n",
    "    plant = station.GetSubsystemByName(\"plant\")\n",
    "    ee_pose = builder.AddSystem(\n",
    "        ExtractBodyPose(\n",
    "            station.GetOutputPort(\"body_poses\"),\n",
    "            plant.GetBodyByName(\"iiwa_link_7\").index(),\n",
    "        )\n",
    "    )\n",
    "    builder.Connect(\n",
    "        station.GetOutputPort(\"body_poses\"), ee_pose.get_input_port()\n",
    "    )\n",
    "    builder.Connect(ee_pose.get_output_port(), teleop.get_input_port())\n",
    "    wsg_teleop = builder.AddSystem(WsgButton(meshcat))\n",
    "    builder.Connect(\n",
    "        wsg_teleop.get_output_port(0), station.GetInputPort(\"wsg.position\")\n",
    "    )\n",
    "\n",
    "    # Simulate.\n",
    "    diagram = builder.Build()\n",
    "    simulator = Simulator(diagram)\n",
    "\n",
    "    if running_as_notebook:  # Then we're not just running as a test on CI.\n",
    "        simulator.set_target_realtime_rate(1.0)\n",
    "        meshcat.AddButton(\"Stop Simulation\", \"Escape\")\n",
    "        print(\"Press Escape to stop the simulation\")\n",
    "        while meshcat.GetButtonClicks(\"Stop Simulation\") < 1:\n",
    "            simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
    "        meshcat.DeleteButton(\"Stop Simulation\")\n",
    "    else:\n",
    "        simulator.set_target_realtime_rate(0)\n",
    "        simulator.AdvanceTo(0.1)\n",
    "\n",
    "\n",
    "teleop_2d()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "020103ab",
   "metadata": {
    "colab_type": "text",
    "id": "4cTkwpJU8tGX"
   },
   "source": [
    "# Teleop Example (3D)\n",
    "\n",
    "The physics and geometry engines running in the simulation above are actually running in 3D.  This example is almost identical, but we'll use the (default) 3D visualization and add more sliders for controlling the full `roll`, `pitch`, `yaw` angles and `x`, `y`, `z` positions of the end effector."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "493c9e9d",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "8wj7ZlyEw_AB"
   },
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_directives:\n",
    "    file: package://manipulation/clutter.dmd.yaml\n",
    "- add_model:\n",
    "    name: foam_brick\n",
    "    file: package://manipulation/hydro/061_foam_brick.sdf\n",
    "    default_free_body_pose:\n",
    "        base_link:\n",
    "            translation: [0, -0.6, 0.2]\n",
    "model_drivers:\n",
    "    iiwa: !IiwaDriver\n",
    "      hand_model_name: wsg\n",
    "    wsg: !SchunkWsgDriver {}\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "def teleop_3d():\n",
    "    meshcat.ResetRenderMode()\n",
    "\n",
    "    builder = DiagramBuilder()\n",
    "\n",
    "    scenario = load_scenario(data=scenario_data)\n",
    "    station = builder.AddSystem(MakeHardwareStation(scenario, meshcat))\n",
    "    # TODO(russt): Replace with station.AddDiffIk(...)\n",
    "    controller_plant = station.GetSubsystemByName(\n",
    "        \"iiwa.controller\"\n",
    "    ).get_multibody_plant_for_control()\n",
    "    # Set up differential inverse kinematics.\n",
    "    differential_ik = AddIiwaDifferentialIK(\n",
    "        builder,\n",
    "        controller_plant,\n",
    "        frame=controller_plant.GetFrameByName(\"iiwa_link_7\"),\n",
    "    )\n",
    "    builder.Connect(\n",
    "        differential_ik.get_output_port(),\n",
    "        station.GetInputPort(\"iiwa.position\"),\n",
    "    )\n",
    "    builder.Connect(\n",
    "        station.GetOutputPort(\"iiwa.state_estimated\"),\n",
    "        differential_ik.GetInputPort(\"robot_state\"),\n",
    "    )\n",
    "\n",
    "    # Set up teleop widgets.\n",
    "    meshcat.DeleteAddedControls()\n",
    "    teleop = builder.AddSystem(\n",
    "        MeshcatPoseSliders(\n",
    "            meshcat,\n",
    "            lower_limit=[0, -0.5, -np.pi, -0.6, -0.8, 0.0],\n",
    "            upper_limit=[2 * np.pi, np.pi, np.pi, 0.8, 0.3, 1.1],\n",
    "        )\n",
    "    )\n",
    "    builder.Connect(\n",
    "        teleop.get_output_port(), differential_ik.GetInputPort(\"X_WE_desired\")\n",
    "    )\n",
    "    # Note: This is using \"Cheat Ports\". For it to work on hardware, we would\n",
    "    # need to construct the initial pose from the HardwareStation outputs.\n",
    "    plant = station.GetSubsystemByName(\"plant\")\n",
    "    ee_pose = builder.AddSystem(\n",
    "        ExtractBodyPose(\n",
    "            station.GetOutputPort(\"body_poses\"),\n",
    "            plant.GetBodyByName(\"iiwa_link_7\").index(),\n",
    "        )\n",
    "    )\n",
    "    builder.Connect(\n",
    "        station.GetOutputPort(\"body_poses\"), ee_pose.get_input_port()\n",
    "    )\n",
    "    builder.Connect(ee_pose.get_output_port(), teleop.get_input_port())\n",
    "    wsg_teleop = builder.AddSystem(WsgButton(meshcat))\n",
    "    builder.Connect(\n",
    "        wsg_teleop.get_output_port(0), station.GetInputPort(\"wsg.position\")\n",
    "    )\n",
    "\n",
    "    diagram = builder.Build()\n",
    "    simulator = Simulator(diagram)\n",
    "    simulator.get_mutable_context()\n",
    "\n",
    "    if running_as_notebook:  # Then we're not just running as a test on CI.\n",
    "        simulator.set_target_realtime_rate(1.0)\n",
    "\n",
    "        meshcat.AddButton(\"Stop Simulation\", \"Escape\")\n",
    "        print(\"Press Escape to stop the simulation\")\n",
    "        while meshcat.GetButtonClicks(\"Stop Simulation\") < 1:\n",
    "            simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
    "        meshcat.DeleteButton(\"Stop Simulation\")\n",
    "\n",
    "    else:\n",
    "        simulator.AdvanceTo(0.1)\n",
    "\n",
    "\n",
    "teleop_3d()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ea72d1ac",
   "metadata": {},
   "source": [
    "# Teleop with a GamePad\n",
    "\n",
    "If you have a [gamepad](https://gamepad-tester.com/) connected to your computer, then you can press a button with the meshcat window in focus to activate gamepad support. Here is the demo again with the gamepad interface.\n",
    "\n",
    "```\n",
    "END EFFECTOR CONTROL\n",
    "-----------------------------------------\n",
    "+/- x-axis         - leftjoy left / right\n",
    "+/- y-axis         - leftjoy up / down\n",
    "+/- roll           - rightjoy up / down\n",
    "+/- pitch          - rightjoy left / right\n",
    "+/- z-axis         - l2 / r2\n",
    "+/- yaw            - l1 / r1\n",
    "\n",
    "GRIPPER CONTROL\n",
    "-----------------------------------------\n",
    "open / close       - square / circle (O)\n",
    "\n",
    "-----------------------------------------\n",
    "x button           - quit\n",
    "```"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6705c047",
   "metadata": {},
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_directives:\n",
    "    file: package://manipulation/clutter.dmd.yaml\n",
    "- add_model:\n",
    "    name: foam_brick\n",
    "    file: package://manipulation/hydro/061_foam_brick.sdf\n",
    "    default_free_body_pose:\n",
    "        base_link:\n",
    "            translation: [0, -0.6, 0.2]\n",
    "model_drivers:\n",
    "    iiwa: !IiwaDriver\n",
    "      hand_model_name: wsg\n",
    "    wsg: !SchunkWsgDriver {}\n",
    "\"\"\"\n",
    "\n",
    "\n",
    "# TODO(russt): Clean this up and move it to a .py file.\n",
    "class GamepadDiffIK(LeafSystem):\n",
    "    def __init__(self, meshcat, plant, frame_E):\n",
    "        \"\"\"\n",
    "        Args:\n",
    "            meshcat: A Meshcat instance.\n",
    "            plant: A multibody plant (to use for differential ik). It is\n",
    "              probably the plant used for control, not for simulation (it should only contain the robot, not the objects).\n",
    "            frame: A frame in to control `plant`.\n",
    "        \"\"\"\n",
    "        LeafSystem.__init__(self)\n",
    "\n",
    "        self.DeclareVectorInputPort(\n",
    "            \"robot_state\", plant.num_multibody_states()\n",
    "        )\n",
    "        self.DeclareInitializationDiscreteUpdateEvent(self.Initialize)\n",
    "\n",
    "        port = self.DeclareVectorOutputPort(\n",
    "            \"iiwa.position\", plant.num_positions(), self.OutputIiwaPosition\n",
    "        )\n",
    "        # The gamepad has undeclared state.  For now, we accept it,\n",
    "        # and simply disable caching on the output port.\n",
    "        port.disable_caching_by_default()\n",
    "\n",
    "        port = self.DeclareVectorOutputPort(\n",
    "            \"wsg.position\", 1, self.OutputWsgPosition\n",
    "        )\n",
    "\n",
    "        self.DeclareDiscreteState(plant.num_positions())  # iiwa position\n",
    "        self.DeclareDiscreteState(1)  # wsg position\n",
    "        self.DeclareDiscreteState(1)  # gripper button pressed\n",
    "        self._time_step = 0.05\n",
    "        self.DeclarePeriodicDiscreteUpdateEvent(\n",
    "            self._time_step, 0, self.Integrate\n",
    "        )\n",
    "\n",
    "        self._meshcat = meshcat\n",
    "        self._plant = plant\n",
    "        self._plant_context = plant.CreateDefaultContext()\n",
    "\n",
    "        if frame_E is None:\n",
    "            frame_E = plant.GetFrameByName(\"body\")  # wsg gripper frame\n",
    "        self._frame_E = frame_E\n",
    "\n",
    "        params = DifferentialInverseKinematicsParameters(\n",
    "            plant.num_positions(), plant.num_velocities()\n",
    "        )\n",
    "        q0 = plant.GetPositions(plant.CreateDefaultContext())\n",
    "        params.set_time_step(self._time_step)\n",
    "        params.set_nominal_joint_position(q0)\n",
    "        params.set_end_effector_angular_speed_limit(2)\n",
    "        params.set_end_effector_translational_velocity_limits(\n",
    "            [-2, -2, -2], [2, 2, 2]\n",
    "        )\n",
    "        iiwa14_velocity_limits = np.array([1.4, 1.4, 1.7, 1.3, 2.2, 2.3, 2.3])\n",
    "        params.set_joint_velocity_limits(\n",
    "            (-iiwa14_velocity_limits, iiwa14_velocity_limits)\n",
    "        )\n",
    "        params.set_joint_centering_gain(10 * np.eye(7))\n",
    "\n",
    "        self._diff_ik_params = params\n",
    "        self._wsg_open = 0.107\n",
    "        self._wsg_close = 0.002\n",
    "\n",
    "    def Initialize(self, context, discrete_state):\n",
    "        discrete_state.set_value(\n",
    "            0,\n",
    "            self.get_input_port().Eval(context)[: self._plant.num_positions()],\n",
    "        )\n",
    "        discrete_state.set_value(1, [self._wsg_open])\n",
    "        discrete_state.set_value(2, [0])\n",
    "\n",
    "    def Integrate(self, context, discrete_state):\n",
    "        gamepad = self._meshcat.GetGamepad()\n",
    "\n",
    "        # https://beej.us/blog/data/javascript-gamepad/\n",
    "        def CreateStickDeadzone(x, y):\n",
    "            stick = np.array([x, y])\n",
    "            deadzone = 0.3\n",
    "            m = np.linalg.norm(stick)\n",
    "            if m < deadzone:\n",
    "                return np.array([0, 0])\n",
    "            over = (m - deadzone) / (1 - deadzone)\n",
    "            return stick * over / m\n",
    "\n",
    "        left = CreateStickDeadzone(gamepad.axes[0], gamepad.axes[1])\n",
    "        right = CreateStickDeadzone(gamepad.axes[2], gamepad.axes[3])\n",
    "\n",
    "        V_WE_desired = np.zeros((6,))\n",
    "        # TODO(russt): Properly implement rpydot to angular velocity.\n",
    "        V_WE_desired[0] = -0.2 * right[0]  # Right stick x => wx\n",
    "        V_WE_desired[1] = 0.2 * right[0]  # Right stick y => wy\n",
    "        if gamepad.button_values[4] > 0.2 or gamepad.button_values[5] > 0.2:\n",
    "            # l1/r1 => wz\n",
    "            V_WE_desired[2] = 0.2 * (\n",
    "                gamepad.button_values[5] - gamepad.button_values[4]\n",
    "            )\n",
    "        V_WE_desired[3] = -0.2 * left[0]  # Left stick x => vx\n",
    "        V_WE_desired[4] = 0.2 * left[1]  # Left stick y => vy\n",
    "        if gamepad.button_values[6] > 0.2 or gamepad.button_values[7] > 0.2:\n",
    "            # l2/r2 => vx\n",
    "            V_WE_desired[5] = 0.2 * (\n",
    "                gamepad.button_values[7] - gamepad.button_values[6]\n",
    "            )\n",
    "\n",
    "        q = np.copy(context.get_discrete_state(0).get_value())\n",
    "        self._plant.SetPositions(self._plant_context, q)\n",
    "        result = DoDifferentialInverseKinematics(\n",
    "            self._plant,\n",
    "            self._plant_context,\n",
    "            V_WE_desired,\n",
    "            self._frame_E,\n",
    "            self._diff_ik_params,\n",
    "        )\n",
    "        if (\n",
    "            result.status\n",
    "            != DifferentialInverseKinematicsStatus.kNoSolutionFound\n",
    "        ):\n",
    "            discrete_state.set_value(\n",
    "                0, q + self._time_step * result.joint_velocities\n",
    "            )\n",
    "\n",
    "        # Toggle gripper open/close based on buttons\n",
    "        x_pressed = context.get_discrete_state(2).get_value()[0]\n",
    "        if gamepad.button_values[1] > 0.5:\n",
    "            if not x_pressed:\n",
    "                wsg_position = context.get_discrete_state(1).get_value()[0]\n",
    "                if wsg_position == self._wsg_open:\n",
    "                    discrete_state.set_value(1, [self._wsg_close])\n",
    "                else:\n",
    "                    discrete_state.set_value(1, [self._wsg_open])\n",
    "            discrete_state.set_value(2, [1])\n",
    "        else:\n",
    "            discrete_state.set_value(2, [0])\n",
    "\n",
    "        # TODO(russt): This doesn't actually work yet, since the event status\n",
    "        # is being discarded in the pybind later.\n",
    "        if gamepad.button_values[0] > 0.5:\n",
    "            return EventStatus.ReachedTermination(self, \"x button pressed\")\n",
    "\n",
    "        return EventStatus.Succeeded()\n",
    "\n",
    "    def OutputIiwaPosition(self, context, output):\n",
    "        output.set_value(context.get_discrete_state(0).get_value())\n",
    "\n",
    "    def OutputWsgPosition(self, context, output):\n",
    "        output.set_value(context.get_discrete_state(1).get_value())\n",
    "\n",
    "\n",
    "def gamepad_teleop():\n",
    "    if meshcat.GetGamepad().index == None:\n",
    "        print(\n",
    "            \"Press a button on the gamepad with the meshcat window in focus to activate gamepad support (then run this cell again).\"\n",
    "        )\n",
    "        return\n",
    "\n",
    "    builder = DiagramBuilder()\n",
    "\n",
    "    scenario = load_scenario(data=scenario_data)\n",
    "    station = builder.AddSystem(MakeHardwareStation(scenario, meshcat))\n",
    "    controller_plant = station.GetSubsystemByName(\n",
    "        \"iiwa.controller\"\n",
    "    ).get_multibody_plant_for_control()\n",
    "    meshcat.ResetRenderMode()\n",
    "    meshcat.DeleteAddedControls()\n",
    "\n",
    "    frame = controller_plant.GetFrameByName(\"iiwa_link_7\")\n",
    "    gamepad = builder.AddSystem(\n",
    "        GamepadDiffIK(meshcat, controller_plant, frame)\n",
    "    )\n",
    "    builder.Connect(\n",
    "        gamepad.GetOutputPort(\"iiwa.position\"),\n",
    "        station.GetInputPort(\"iiwa.position\"),\n",
    "    )\n",
    "    builder.Connect(\n",
    "        gamepad.GetOutputPort(\"wsg.position\"),\n",
    "        station.GetInputPort(\"wsg.position\"),\n",
    "    )\n",
    "    builder.Connect(\n",
    "        station.GetOutputPort(\"iiwa.state_estimated\"),\n",
    "        gamepad.GetInputPort(\"robot_state\"),\n",
    "    )\n",
    "\n",
    "    diagram = builder.Build()\n",
    "    simulator = Simulator(diagram)\n",
    "    simulator.get_mutable_context()\n",
    "\n",
    "    simulator.set_target_realtime_rate(1.0)\n",
    "\n",
    "    meshcat.AddButton(\"Stop Simulation\", \"Escape\")\n",
    "    print(\"Press Escape to stop the simulation\")\n",
    "    while meshcat.GetButtonClicks(\"Stop Simulation\") < 1:\n",
    "        simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)\n",
    "    meshcat.DeleteButton(\"Stop Simulation\")\n",
    "\n",
    "\n",
    "gamepad_teleop()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.10 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python",
   "version": "3.11.4"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
